

using namespace std;

#include <iostream>
#include "a_star.h"
#include <vector>
#include <queue>
#include <algorithm>

Node::Node() {
	this->bestParent = NULL;
	this->g = 0;
	this->h = 0;
	this->isSolution = false;
}
Node::Node(int g, int h) {
	this->bestParent = NULL;
	this->g = g;
	this->h = h;
	this->isSolution = false;
	calcF();
}
double Node::getG(void) {
	return this->g;
}
double Node::getH(void) {
	return this->h;
}
double Node::getF(void) {
	return this->f;
}
void Node::setG(int g) {
	this->g = g;
	calcF();
}
void Node::setH(int h) {
	this->h = h;
	calcF();
}
void Node::calcF(void) {
	this->f = this->g + this->h;
}
void Node::setParent(Node* parent) {
	this->bestParent = parent;
}
Node* Node::getBestParent(void) {
	return this->bestParent;
}
void Node::insertChild(Node* newChild) {
	this->children.push_back(newChild);
}
vector<Node*> Node::getChildren(void) {
	return children;
}
bool Node::getIsSolution(void){
	return isSolution;
}
double Node::heuristicFunc(void) {
	return 10;
}
void Node::makeNodeChildren(void) {
}

bool operator < (const Node& n1, const Node& n2) { 
	return n1.f > n2.f; // Swapped so that priority queue works with the lowest on top
}

string reconstructPath(void){
	return "GOOOOOOOOAAAAAAAAAAL!";
}

Node* aStarPath (int searchProblem, int herusticFuncH) {
	// Create open_node_list as two priority queues. Two because than you can alter the priority of a given node.
	//priority_queue<Node> openSet[2]
	priority_queue<Node*, vector<Node*>, CompareNode> openSet[2];
	// Index. Which of the lists are active
	int openSetIndex = 0;
	//A vector containing all the closed nodes
	vector<Node*> closedSet;

	// Create start node
	Node* startNode = new Node(0,0); // SETH()? From the heuristic function sent in?
	// Insert startNode into priority queue
	openSet[openSetIndex].push(startNode);

	while(!openSet[openSetIndex].empty()) {
		// Read top node from openSet with the lowest f
		Node* currentNode = openSet[openSetIndex].top();
		//Check if current is the solution
		if(currentNode->getIsSolution()){
			return currentNode;
		}
		//Remove current from openset
		openSet[openSetIndex].pop();
		//Add current to closedset
		closedSet.push_back(currentNode);
		//For each nabour of current
		vector<Node*> tempNeighbors = currentNode->getChildren();
		for(int i = 0; i < tempNeighbors.size(); i++) {
			//If neighbour in closedset
			for(int j = 0; j < closedSet.size(); j++) {
				if (tempNeighbors[i] == closedSet[j]) {
					//This neighbour is in the closedSet, continue?
					break;
				}
				else {
					//Node is not in closed set
					//Compute h'(m) for neighbor
					//Compute tentative g of neighbor g'(m) = g(n) + c(m,n), cost to neighbor m = cost to current + cost from current to neighbor
					//Compute tentative f of neighbor f'(m) = g'(m) + h'(m)
					//If m(neighbor) is not in closeset or openset
						//Insert m into openset with priority f(m) = f'(m)
					//else if m is in openset and tentative f'(m) < f(m), ie better f
						//Promote m in openset to priority f(m) = f'(m)
					//else if m is in closedset and f'(m) < f(m)
						//re open m in openset with priority f(m) = f'(m)
					//Insert n into closedset with priority f(n)
					
				}
			}
		}
		while(1);
	}
	return NULL; //Failed!
}






